#include "Com_PID.h"

// 函数：计算PID控制器的输出
// 参数：pid：PID控制器的结构体指针；dt：采样时间间隔
void Com_PID_Calculate(PID_Struct *pid, float dt)
{
    // 计算误差
    float error = pid->measure - pid->desire;
    // 计算积分项
    pid->integral += error * dt;
    // 计算输出
    pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * (error - pid->last_error) / dt;
    // 更新上一次的误差
    pid->last_error = error;
}

// 函数：Com_PID_CascadeCalculate
// 功能：级联PID计算
// 参数：inner：内环PID结构体指针；outter：外环PID结构体指针；dt：时间间隔
void Com_PID_CascadeCalculate(PID_Struct *inner, PID_Struct *outter, float dt)
{
    //  PID_Struct pid_outter_last = {0};
    // 调用Com_PID_Calculate函数计算外环PID
    Com_PID_Calculate(outter, dt);
    // 将外环PID的输出值赋给内环PID的期望值
    //  inner->desire = LIMIT1((outter->output - pid_outter_last.output)/0.06, -2000, 2000);
     inner->desire = outter->output;
    
    // pid_outter_last = *outter;
   
    // 调用Com_PID_Calculate函数计算内环PID
    Com_PID_Calculate(inner, dt);
}
